/*
 Now problem
 1. April tag is too far away, error is big
 2. Stop too close, can't see whole tag
 */

#include "ros/ros.h"
#include "tf/transform_broadcaster.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
#include "std_msgs/String.h"
#include "nav_msgs/OccupancyGrid.h"
#include "mother_ship/ModeControl.h"
#include "std_msgs/String.h"
#include <vector>
#include <sstream>
#include <fstream>
using namespace std;

#define PI 3.1415926

struct single_pose {
	double x, y, z, roll, pitch, yaw, turn_angle;
};
struct tag_link {
	double dist, angle;
	int begin, end;
};

#ifndef TF_MATRIX3x3_H
typedef btScalar tfScalar;
namespace tf {typedef btMatrix3x3 Matrix3x3;}
#endif

ros::NodeHandle *n;
tf::TransformBroadcaster* tfB_;
tf::StampedTransform transform_;
tf::Quaternion tmp_;
ros::Subscriber april_sub;
ros::Subscriber mag_sub;
ros::Subscriber route_sub;
ros::Publisher mode_control_pub;
ros::Publisher navigation_state_pub;
std_msgs::String nav_msg;
mother_ship::ModeControl mode_msg;

// map frame
std::string map_frame_;
// april tag frame relative to map frame
std::string april_translation_frame_, april_frame_;
// robot frame relative to april tag
std::string robot_translation_frame_, robot_frame_;

// april pose filter
ros::Time last_stamp;
int last_april_id;

// initialization
void getParameters();
void initialization();

// map construction
ros::Subscriber map_sub;
void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg);
int map_width, map_height, map_resolution;
bool visualize_all;
void visualizeAllAprilPose();
int april_num, link_num;
vector<single_pose> aprils_pose;
vector<vector<double> > edges;
vector<tag_link> links;
// planning
single_pose start_pose;
int final_april_id, start_april_id;
vector<single_pose> goal_seq;
vector<int> aprils_id, aprils_id_extra;
vector<int> goal_april_id_seq, goal_april_id_seq_extra;
vector<double> turn_dist_seq;
vector<double> angle2next_goal, turn_dist, track_dist;
ros::Time now_tracking_stamp;

bool findShortestPath();
int current_april_index;
single_pose current_relative_pose, current_pose;
float current_global_direction;
double start_to_turn_dist, start_to_track_dist;
bool is_navigating;

// localization
void mag_Callback(const std_msgs::Float32::ConstPtr &msg);
void aprilCallback(const geometry_msgs::PoseStamped& msg_stamped);
void pubTransforms(const geometry_msgs::Pose& msg, int april_id);
void route_Callback(const std_msgs::Int32::ConstPtr &msg);
double localization_error;

// mode control
bool corridow_following, turning, tagtracking;
bool mag_updated;
bool use_mag;

// utils
double dist2D(single_pose pose1, single_pose pose2) {
	return std::sqrt((pose1.x - pose2.x) * (pose1.x - pose2.x) + (pose1.y
			- pose2.y) * (pose1.y - pose2.y));
}

int main(int argc, char **argv) {
	ros::init(argc, argv, "navigation");

	n = new ros::NodeHandle;

	getParameters();
	initialization();

	tfB_ = new tf::TransformBroadcaster();
	april_sub = n->subscribe("april_pose", 1, aprilCallback);
	mag_sub = n->subscribe<std_msgs::Float32> ("Magnetometer", 1, mag_Callback);
	mode_control_pub = n->advertise<mother_ship::ModeControl> ("mode_control",
			5);
	navigation_state_pub = n->advertise<std_msgs::String> ("Navigation_State",
			10);
	route_sub = n->subscribe<std_msgs::Int32> ("Route", 1, route_Callback);

	// debug
	//while (true) {
	//	visualizeAllAprilPose();
	//	sleep(3);
	//	ros::spinOnce();
	//}

	// start to go
	if (!use_mag) {
		ROS_INFO("Start to go to first April Tag");
		nav_msg.data = "Start to go to first April Tag";
		navigation_state_pub.publish(nav_msg);
		mode_msg.mode = "CorridorFollowing";
		mode_msg.desired_global_angle = 90.0;
		// look for next april tag
		mode_control_pub.publish(mode_msg);
		mag_updated = true;
	}

	if (!findShortestPath()) {
		delete tfB_;
		return 0;
	}

	ros::spin();

	delete tfB_;

	return 0;
}
void getParameters() {
	// get frame name
	n->param("robot_global_frame", robot_frame_, std::string(
			"/robot_global_pose"));
	n->param("robot_translation_frame", robot_translation_frame_, std::string(
			"/robot_translation"));
	n->param("april_ranslation_frame", april_translation_frame_, std::string(
			"/april_global_translation"));
	n->param("april_global_frame", april_frame_, std::string(
			"/april_global_pose"));
	n->param("map_frame", map_frame_, std::string("/map"));

	// get april tags properties
	n->param("visualize_all_aprils", visualize_all, false);
	n->param("april_tags_number", april_num, 2);
	n->param("link_number", link_num, 2);
	n->param("start_april_id", start_april_id, 0);
	n->param("final_april_id", final_april_id, 0);
	n->param("track_apriltag", tagtracking, false);

	// get start point pose
	n->param("start_translation_x", start_pose.x, 0.0);
	n->param("start_translation_y", start_pose.y, 0.0);
	n->param("start_translation_z", start_pose.z, 0.0);
	n->param("start_roll", start_pose.roll, 0.0);
	n->param("start_pitch", start_pose.pitch, 0.0);
	n->param("start_yaw", start_pose.yaw, 0.0);

	// get april tags pose
	aprils_pose.resize(april_num);
	aprils_id.resize(april_num);
	aprils_id_extra.resize(april_num);
	turn_dist.resize(april_num);
	track_dist.resize(april_num);
	for (int i = 0; i < april_num; i++) {
		stringstream sstr;
		string str, property_name;
		sstr << i;
		sstr >> str;
		property_name = "april_" + str + "_id";
		n->param(property_name, aprils_id[i], 0);
		property_name = "april_" + str + "_id_extra";
		n->param(property_name, aprils_id_extra[i], 0);
		if (aprils_id_extra[i] == 0)
			aprils_id_extra[i] = aprils_id[i];
		property_name = "april_" + str + "_translation_x";
		n->param(property_name, aprils_pose[i].x, 0.0);
		property_name = "april_" + str + "_translation_y";
		n->param(property_name, aprils_pose[i].y, 0.0);
		property_name = "april_" + str + "_translation_z";
		n->param(property_name, aprils_pose[i].z, 0.0);
		property_name = "april_" + str + "_turn_dist";
		n->param(property_name, turn_dist[i], 3.0);
		property_name = "april_" + str + "_track_dist";
		n->param(property_name, track_dist[i], 4.0);
		tfScalar yaw, pitch, roll;
		property_name = "april_" + str + "_roll";
		n->param(property_name, aprils_pose[i].roll, 0.0);
		aprils_pose[i].roll = aprils_pose[i].roll / 180.0 * PI;
		property_name = "april_" + str + "_pitch";
		n->param(property_name, aprils_pose[i].pitch, 0.0);
		aprils_pose[i].pitch = aprils_pose[i].pitch / 180.0 * PI;
		property_name = "april_" + str + "_yaw";
		n->param(property_name, aprils_pose[i].yaw, 0.0);
		aprils_pose[i].yaw = aprils_pose[i].yaw / 180.0 * PI;
		property_name = "april_" + str + "_turn_angle";
		n->param(property_name, aprils_pose[i].turn_angle, 0.0);
	}

	links.resize(link_num);
	for (int i = 0; i < link_num; i++) {
		stringstream sstr;
		string str, property_name;
		sstr << i;
		sstr >> str;
		property_name = "line_" + str + "_begin";
		n->param(property_name, links[i].begin, 0);
		property_name = "line_" + str + "_end";
		n->param(property_name, links[i].end, 0);
		property_name = "line_" + str + "_angle";
		n->param(property_name, links[i].angle, 0.0);
	}

	n->param("start_to_turn_dist", start_to_turn_dist, 1.0);
	n->param("start_to_track_dist", start_to_track_dist, 1.0);
}
void initialization() {
	//for (int i = 0; i < april_num; i++) {
	//	goal_seq.push_back(aprils_pose[i]);
	//	goal_april_id_seq.push_back(aprils_id[i]);
	//		angle2next_goal.push_back(aprils_pose[i].turn_angle);
	//	}

	current_april_index = -1;
	mag_updated = false;
	use_mag = false;
	localization_error = -1;
	is_navigating = true;
	now_tracking_stamp = ros::Time::now();
}
void route_Callback(const std_msgs::Int32::ConstPtr &msg) {
	start_april_id = msg->data / 100;
	final_april_id = msg->data % 100;
	ROS_INFO("Try to Start a new route from %d to %d", start_april_id, final_april_id);
	nav_msg.data = "Try to Start a new route";
	navigation_state_pub.publish(nav_msg);
	if (!findShortestPath()) {
		return;
	}
	initialization();
}
void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg) {

	//ROS_INFO("Got a frame %d with %d * %d map with resolution %f", frameid % 4, msg->info.height, msg->info.width, msg->info.resolution);

	for (int i = 0; i < msg->info.width; i++) {
		//myfile << msg->data[i * msg->info.height + 0];
		for (int j = 1; j < msg->info.height; j++) {
			//myfile << " " << (int32_t)msg->data[i * msg->info.height + j] ;
			//cout << (int32_t)msg->data[i * msg->info.width + j] << " ";
		}
		//myfile << endl;
		//cout << endl;
	}
	//myfile.close();
}

void visualizeAllAprilPose() {
	for (int i = 0; i < april_num; i++) {
		stringstream sstr;
		string str, frame_translation, frame;
		sstr << i;
		sstr >> str;
		frame_translation = "april_translation_" + str;
		frame = "april_" + str;

		// translation
		tmp_.setRPY(0, 0, 0);
		transform_.setRotation(tmp_);
		transform_.getOrigin().setX(aprils_pose[i].x);
		transform_.getOrigin().setY(aprils_pose[i].y);
		transform_.getOrigin().setZ(aprils_pose[i].z);
		transform_.frame_id_ = map_frame_;
		transform_.child_frame_id_ = frame_translation;
		transform_.stamp_ = ros::Time::now();
		tfB_->sendTransform(transform_);

		// rotation
		transform_.getOrigin().setX(0.0);
		transform_.getOrigin().setY(0.0);
		transform_.getOrigin().setZ(0.0);
		tmp_.setRPY(aprils_pose[i].roll, aprils_pose[i].pitch,
				aprils_pose[i].yaw);
		transform_.setRotation(tmp_);
		transform_.frame_id_ = frame_translation;
		transform_.child_frame_id_ = frame;
		transform_.stamp_ = ros::Time::now();
		tfB_->sendTransform(transform_);
	}
}
bool findShortestPath() {
	vector<int> seq, parent;
	vector<bool> flag;
	parent.resize(april_num);
	flag.resize(april_num);
	for (int i = 0; i < april_num; i++)
		flag[i] = false;
	seq.push_back(start_april_id);
	int now_pos = 0;
	parent[start_april_id] = start_april_id;
	flag[start_april_id] = true;
	while (now_pos + 1 <= seq.size()) {
		if (seq[now_pos] == final_april_id)
			break;
		//ROS_INFO("now %d final %d", seq[now_pos], final_april_id);
		for (int i = 0; i < links.size(); i++)
			if (links[i].begin == seq[now_pos] && !flag[links[i].end]) {
				//seq
				flag[links[i].end] = true;
				seq.push_back(links[i].end);
				parent[links[i].end] = links[i].begin;
			}
		now_pos++;
	}
	if (now_pos + 1 > seq.size()) {
		ROS_INFO("Find Path Failed! Cannot Go there, Sorry!");
		nav_msg.data = "Find Path Failed! Cannot Go there, Sorry!";
		navigation_state_pub.publish(nav_msg);
		return false;
	}

	vector<int> route;
	int now_node = final_april_id;
	while (parent[now_node] != now_node) {
		//ROS_INFO("now %d parent %d ", now_node, parent[now_node]);		

		route.push_back(now_node);
		now_node = parent[now_node];

	}
	route.push_back(now_node);

	for (int i = route.size() - 1; i > 0; i--) {
		goal_seq.push_back(aprils_pose[route[i]]);
		goal_april_id_seq.push_back(aprils_id[route[i]]);
		goal_april_id_seq_extra.push_back(aprils_id_extra[route[i]]);
		turn_dist_seq.push_back(route[i]);
		for (int j = 0; j < links.size(); j++)
			if (links[j].begin == route[i] && links[j].end == route[i - 1]) {
				angle2next_goal.push_back(links[j].angle);
				break;
			}

		ROS_INFO("From %d to %d with %lf", aprils_id[route[i]], aprils_id[route[i - 1]], angle2next_goal.back());
	}
	goal_seq.push_back(aprils_pose[route[0]]);
	goal_april_id_seq.push_back(aprils_id[route[0]]);
	goal_april_id_seq_extra.push_back(aprils_id_extra[route[0]]);
	turn_dist_seq.push_back(route[0]);

	return true;

	//	for (int i = 0; i < april_num; i++) {
	//		goal_seq.push_back(aprils_pose[i]);
	//		goal_april_id_seq.push_back(aprils_id[i]);
	//		angle2next_goal.push_back(aprils_pose[i].turn_angle);
	//	}
}

void mag_Callback(const std_msgs::Float32::ConstPtr &msg) {
	if (!use_mag)
		return;
	current_global_direction = msg->data;
	if (!mag_updated) {
		sleep(3);
		ROS_INFO("Start to go to first April Tag");
		nav_msg.data = "Start to go to first April Tag";
		navigation_state_pub.publish(nav_msg);
		mode_msg.mode = "CorridorFollowing";
		mode_msg.desired_global_angle = msg->data;
		current_april_index++;
		mode_control_pub.publish(mode_msg);
		mag_updated = true;
	}
}
void aprilCallback(const geometry_msgs::PoseStamped& msg_stamped) {

	if (visualize_all)
		visualizeAllAprilPose();

	if (!is_navigating)
		return;

	// find to april tag pose in global
	istringstream iss(msg_stamped.header.frame_id);
	int real_id, april_id = -1;
	iss >> real_id;
	for (int i = 0; i < aprils_id.size(); i++)
		if (real_id == aprils_id[i]) {
			april_id = i;
			break;
		}

	//ROS_INFO("Found Tag Number %d Searching for Tag Number %d", real_id, goal_april_id_seq[current_april_index + 1]);

	geometry_msgs::Pose msg = msg_stamped.pose;
	// if needed
	if (visualize_all)
		pubTransforms(msg, april_id);

	// update current_relative_pose
	current_relative_pose.x = msg_stamped.pose.position.x;
	current_relative_pose.y = msg_stamped.pose.position.y;
	current_relative_pose.z = msg_stamped.pose.position.z;
	tf::quaternionMsgToTF(msg_stamped.pose.orientation, tmp_);
	tfScalar yaw, pitch, roll;
	tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw);
	current_relative_pose.roll = roll;
	current_relative_pose.pitch = pitch;
	current_relative_pose.yaw = yaw;

	//	mode : "CorridorFollowing", "Turn", "Arrived"
	double relative_dist = std::sqrt(current_relative_pose.x
			* current_relative_pose.x + current_relative_pose.y
			* current_relative_pose.y);
	// if arrive destination
	if (april_id == final_april_id) {
		// stop everything
		if (relative_dist > start_to_turn_dist) {
			ROS_INFO("Found Destination %d But Not Close Enough", april_id);
			nav_msg.data = "Found Destination April Tag But Not Close Enough";
			navigation_state_pub.publish(nav_msg);
			return;
		}
		ROS_INFO("Found Final April Tag %d and Ready to Stop", april_id);
		nav_msg.data = "Found Final April Tag!!!";
		navigation_state_pub.publish(nav_msg);
		mode_msg.mode = "Arrived";
		is_navigating = false;
		mode_control_pub.publish(mode_msg);
		return;
	}
	// if arrive next goal
	if (real_id == goal_april_id_seq[current_april_index + 1] || real_id == goal_april_id_seq_extra[current_april_index + 1]) {

		if (now_tracking_stamp == msg_stamped.header.stamp)
			return;
		else
			now_tracking_stamp = msg_stamped.header.stamp;
		// if error small
		// localization_error smaller than

		//double relative_dist = std::sqrt((current_relative_pose.x
		//		* current_relative_pose.x + current_relative_pose.y
		//		* current_relative_pose.y));

		ROS_INFO("Relative Position x:%.2lf y:%.2lf z:%.2lf", current_relative_pose.x, current_relative_pose.y, current_relative_pose.z);

		// stop and turn
		if (relative_dist < turn_dist[turn_dist_seq[current_april_index + 1]]) {
			ROS_INFO("Found April Tag %d closer than %.2lf and Ready to Turn", turn_dist[turn_dist_seq[current_april_index + 1]], real_id);
			nav_msg.data = "Found Next April Tag and Ready to Turn";
			navigation_state_pub.publish(nav_msg);
			mode_msg.mode = "Turn";

			//determine turn left or right
			mode_msg.desired_relative_angle
					= angle2next_goal[current_april_index + 1];
			if (!use_mag)
				mode_msg.desired_global_angle = 0.0;
			else
				mode_msg.desired_global_angle = current_global_direction
						- current_relative_pose.yaw
						+ mode_msg.desired_relative_angle;

			current_april_index++;
			mode_control_pub.publish(mode_msg);
			return;
		}
		if (tagtracking && relative_dist < track_dist[current_april_index + 1]) { //start_to_track_dist) {
			ROS_INFO("Tracking April Tag %d", real_id);
			nav_msg.data = "Tracking Next April Tag";
			navigation_state_pub.publish(nav_msg);
			mode_msg.mode = "TagTracking";
			mode_msg.desired_relative_angle = -std::atan(
					current_relative_pose.y / current_relative_pose.x) / PI
					* 180.0;
			ROS_INFO("Desired relative angle %lf", mode_msg.desired_relative_angle);
			mode_control_pub.publish(mode_msg);
		} else {
			ROS_INFO("Found April Tag %d But Not Close Enough", april_id);
			nav_msg.data = "Found Next April Tag But Not Close Enough";
			navigation_state_pub.publish(nav_msg);
		}
	} else if (real_id != goal_april_id_seq[current_april_index]
			&& (current_april_index > 0 && real_id
					!= goal_april_id_seq[current_april_index - 1])) {
		ROS_INFO("Found April Tag %d Which is Wrong Place!", aprils_id[april_id]);
		//nav_msg.data
		//		= "Found Wrong April Tag!";
		//navigation_state_pub.publish(nav_msg);

		return;

		is_navigating = false;
		mode_msg.mode = "Stop";
		mode_control_pub.publish(mode_msg);
	}
	// if goes to the wrong place
	// start to plan again
}
void pubTransforms(const geometry_msgs::Pose& msg, int april_id) {
	stringstream sstr;
	string str, frame;
	sstr << april_id;
	sstr >> str;
	frame = "april_" + str;

	tmp_.setRPY(0, 0, 0);
	transform_.setRotation(tmp_);
	transform_.getOrigin().setX(0.0);
	transform_.getOrigin().setY(0.0);
	transform_.getOrigin().setZ(0.0);
	transform_.frame_id_ = frame;
	transform_.child_frame_id_ = april_frame_;
	tfB_->sendTransform(transform_);

	// find pose relative to april in global
	tmp_.setRPY(0, 0, 0);
	transform_.setRotation(tmp_);
	transform_.getOrigin().setX(-msg.position.x);
	transform_.getOrigin().setY(msg.position.y);
	transform_.getOrigin().setZ(msg.position.z);

	transform_.frame_id_ = robot_translation_frame_;
	transform_.child_frame_id_ = robot_frame_;
	//transform_.stamp_ = msg_stamped.header.stamp;

	tfB_->sendTransform(transform_);

	tf::quaternionMsgToTF(msg.orientation, tmp_);
	tfScalar yaw, pitch, roll;
	tf::Matrix3x3(tmp_).getRPY(pitch, yaw, roll);
	pitch = pitch / std::abs(pitch) * PI - pitch;
	yaw = yaw / std::abs(yaw) * PI - yaw;

	tmp_.setRPY(-roll, -pitch, -yaw);
	transform_.getOrigin().setX(0.0);
	transform_.getOrigin().setY(0.0);
	transform_.getOrigin().setZ(0.0);
	transform_.setRotation(tmp_);

	transform_.frame_id_ = april_frame_;
	transform_.child_frame_id_ = robot_translation_frame_;
	//transform_.stamp_ = msg_stamped.header.stamp;
	tfB_->sendTransform(transform_);
}
